/*
 Copyright 2011 and onwards Joao C. R. Pereira

 Licensed under the Apache License, Version 2.0 (the "License");
 you may not use this file except in compliance with the License.
 You may obtain a copy of the License at

      http://www.apache.org/licenses/LICENSE-2.0

      Unless required by applicable law or agreed to in writing, software
      distributed under the License is distributed on an "AS IS" BASIS,
      WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
      See the License for the specific language governing permissions and
      limitations under the License.
*/

#include <string.h>
#include "BitBuffer.h"
#include "Memory.h"

BitBuffer BitBuffer_New(size_t capacity, size_t factor) {
  
  BitBuffer buffer = NewObj(BitBufferObj);

  STARTSAFEPOINT(buffer);

  buffer->value = 0;
  buffer->pos = 0;
  Buffer_Init(&buffer->buffer, capacity, factor);

  CHECKSAFEPOINTS(buffer);
  
  return buffer;
}

size_t BitBuffer_GetCapacity(BitBuffer buffer) {

  CHECKSAFEPOINTS(buffer);
  return Buffer_GetCapacity(&buffer->buffer);
}

size_t BitBuffer_GetLength(BitBuffer buffer) {

  CHECKSAFEPOINTS(buffer);
  return Buffer_GetLength(&buffer->buffer);
}

void BitBuffer_Clear(BitBuffer buffer) {

  Buffer_Clear(&buffer->buffer);
  buffer->value = 0;
  buffer->pos = 0;
}

void BitBuffer_AddByte(BitBuffer buffer, byte b) {

  BitBuffer_AddBits(buffer, b, 8);
}

void BitBuffer_AddBits(BitBuffer buffer, byte nbits, byte b) {

  CHECKSAFEPOINTS(buffer);

  switch(buffer->pos) {
  case 0:
    switch(nbits) {
    case 1:
      buffer->value = b << 7; buffer->pos =+ nbits;
      break;
    case 2: 
      buffer->value = b << 6; buffer->pos =+ nbits;
      break;
    case 3: 
      buffer->value = b << 5; buffer->pos =+ nbits;
      break;
    case 4: 
      buffer->value = b << 4; buffer->pos =+ nbits;
      break;
    case 5: 
      buffer->value = b << 3; buffer->pos =+ nbits;
      break;
    case 6: 
      buffer->value = b << 2; buffer->pos =+ nbits;
      break;
    case 7: 
      buffer->value = b << 1; buffer->pos =+ nbits;
      break;
    case 8:
      Buffer_AddByte(&buffer->buffer, b);
      buffer->value = 0; 
      break;
    }

    break;
  case 1:
    switch(nbits) {
    case 1: 
      printf("Value before: %u (%u)\n", buffer->value, buffer->pos);
      buffer->value = buffer->value | (b << 6); buffer->pos =+ nbits;
      printf(" Value after: %u (%u)\n", buffer->value, buffer->pos);
      break;
    case 2: 
      buffer->value = buffer->value | (b << 5); buffer->pos =+ nbits;
      break;
    case 3: 
      buffer->value = buffer->value | (b << 4); buffer->pos =+ nbits;
      break;
    case 4: 
      buffer->value = buffer->value | (b << 3); buffer->pos =+ nbits;
      break;
    case 5: 
      buffer->value = buffer->value | (b << 2); buffer->pos =+ nbits;
      break;
    case 6:
       buffer->value = buffer->value | (b << 1); buffer->pos =+ nbits;
      break;
    case 7: 
      buffer->value = buffer->value | ((b && 0b01111111)); Buffer_AddByte(&buffer->buffer, buffer->value); buffer->value = 0;  buffer->pos = 0;
      break;
    case 8: 
      buffer->value = buffer->value | (b >> 1); 
      BitBuffer_AddBits(buffer, (b | 0b00000001), 1);
      break;
    }
    break;
  case 2:
    switch(nbits) {
    case 1: 
      printf("Value before: %u (%u)\n", buffer->value, buffer->pos);
      buffer->value = buffer->value | (b << 5); buffer->pos =+ nbits;
      printf(" Value after: %u (%u)\n", buffer->value, buffer->pos);
      break;
    case 2: 
      buffer->value = buffer->value | (b << 4); buffer->pos =+ nbits;
      break;
    case 3: 
      buffer->value = buffer->value | (b << 3); buffer->pos =+ nbits;
      break;
    case 4: 
      buffer->value = buffer->value | (b << 2); buffer->pos =+ nbits;
      break;
    case 5: 
      buffer->value = buffer->value | (b << 1); buffer->pos =+ nbits;
      break;
    case 6:
      buffer->value = buffer->value | ((b && 0b00111111)); Buffer_AddByte(&buffer->buffer, buffer->value); buffer->value = 0;  buffer->pos = 0;
      break;
    case 7: 
      buffer->value = buffer->value | (b >> 1); Buffer_AddByte(&buffer->buffer, buffer->value); buffer->value = 0;  buffer->pos = 0;
      BitBuffer_AddBits(buffer, (b | 0b00000001), 1);
      break;
    case 8: 
      buffer->value = buffer->value | (b >> 2); Buffer_AddByte(&buffer->buffer, buffer->value); buffer->value = 0;  buffer->pos = 0;
      BitBuffer_AddBits(buffer, (b | 0b00000011), 2);
      break;
    }
    break;
  case 3:
    switch(nbits) {
    case 1:
      printf("Value before: %u (%u)\n", buffer->value, buffer->pos);
      buffer->value = buffer->value | (b << 4); buffer->pos =+ nbits;
      printf(" Value after: %u (%u)\n", buffer->value, buffer->pos);
      break;
    }
  case 4:
    switch(nbits) {
    case 1:
      printf("Value before: %u (%u)\n", buffer->value, buffer->pos);
      buffer->value = buffer->value | (b << 3); buffer->pos =+ nbits;
      printf(" Value after: %u (%u)\n", buffer->value, buffer->pos);
      break;
    }
  case 5:
    switch(nbits) {
    case 1:
      printf("Value before: %u (%u)\n", buffer->value, buffer->pos);
      buffer->value = buffer->value | (b << 2); buffer->pos =+ nbits;
      printf(" Value after: %u (%u)\n", buffer->value, buffer->pos);
      break;
    }
  case 6:
    switch(nbits) {
    case 1:
      printf("Value before: %u (%u)\n", buffer->value, buffer->pos);
      buffer->value = buffer->value | (b << 1); buffer->pos =+ nbits;
      printf(" Value after: %u (%u)\n", buffer->value, buffer->pos);
      break;
    case 7:
      switch(nbits) {
      case 1:
	break;
	
      }
    }
  }
  CHECKSAFEPOINTS(buffer);
  
}

void BitBuffer_AddWord(BitBuffer buffer, word w) {
  
  byte a,b;
  
  CHECKSAFEPOINTS(buffer);
  Assert_IsTrue(w <= MAXWORD);
  a = w / 256;
  b = w - (a*256);

  BitBuffer_AddByte(buffer, a);
  BitBuffer_AddByte(buffer, b);
}

void BitBuffer_Print(BitBuffer buffer) {

  CHECKSAFEPOINTS(buffer);
  Buffer_Print(&buffer->buffer);
}

void BitBuffer_EnsureCapacity(BitBuffer buffer, size_t capacity) {
  
  CHECKSAFEPOINTS(buffer);
 
  Buffer_EnsureCapacity(&buffer->buffer, capacity);
  
  CHECKSAFEPOINTS(buffer);  
}

void BitBuffer_Flush(BitBuffer buffer) {

  Buffer_AddByte(&buffer->buffer, buffer->value);
  buffer->value = 0;
  buffer->pos = 0;
}
